package simpleGeom;

import java.awt.geom.Line2D;

import processing.core.PMatrix3D;

public class MTrianglePreCalc {
	static float SMALL_NUM = 0.00000001f;
	// 0 = disjoint (no intersect)
	// 1 = intersect in unique point I1
	// 2 = are in the same plane
	final static int DISJOINT = 0;
	public final static int INTERSECT = 1;
	final static int SAME_PLANE = 2;

	MSegment l1;
	MSegment l2;
	MSegment l3;

	PMatrix3D m;
	float ux, uy, uz;
	float vx, vy, vz;
	float nx, ny, nz;
	float x1, y1, z1, x2, y2, z2, x3, y3, z3;
	float d0 = 0;// distanceToOrigin;
	float uu, uv, vv;
	float d;

	Vec3D a,b,c;
	 float sd;
	public MTrianglePreCalc() {

	}

	public MTrianglePreCalc(float x1, float y1, float z1, float x2, float y2,
			float z2, float x3, float y3, float z3) {
		this.x1 = x1;
		this.y1 = y1;
		this.z1 = z1;
		this.x2 = x2;
		this.y2 = y2;
		this.z2 = z2;
		this.x3 = x3;
		this.y3 = y3;
		this.z3 = z3;
		
		a=new Vec3D(x1,y1,z1);
		b=new Vec3D(x2,y2,z2);
		c=new Vec3D(x3,y3,z3);
		init();
		// TODO Auto-generated constructor stub
	}

	public float getLength(float vx, float vy, float vz) {
		return (float) Math.sqrt(vx * vx + vy * vy + vz * vz);
	}

	public static float dot(float x1, float y1, float z1, float x2, float y2,
			float z2) {
		return x1 * x2 + y1 * y2 + z1 * z2;
	}

	 public float getAngle(float nx,float ny,float nz, float ax,float ay,float az) {
	        float dot = dot(nx,ny,nz,ax,ay,az);
	        float angle = (float)Math.acos(dot);
	        return (float)angle;
	    }
	
	 public void cross(float x1,float y1,float z1,float x2,float y2,float z2,float[]result){
		 result[0]=y1 * z2 - z1 * y2;
		 result[1] = z1 * x2 - x1 * z2;
		 result[2] = x1 * y2 - y1 * x2;
	 }
	public void init() {
		// calculate v12 & v13
		ux = x2 - x1;
		uy = y2 - y1;
		uz = z2 - z1;
		vx = x3 - x1;
		vy = y3 - y1;
		vz = z3 - z1;

		uu = dot(ux, uy, uz, ux, uy, uz);
		uv = dot(ux, uy, uz, vx, vy, vz);
		vv = dot(vx, vy, vz, vx, vy, vz);
		d = uv * uv - uu * vv;

		// calculate normal
		float[]normal=new float[3];
		cross(ux,uy,uz,vx,vy,vz,normal);
		nx = uy * vz - uz * vy;
		ny = uz * vx - ux * vz;
		nz = ux * vy - uy * vx;
		// normalize
		float mag = getLength(nx, ny, nz);  // could become very small
		//if (mag<0.00000001)System.out.println("very small normal");
		nx /= mag;
		ny /= mag;
		nz /= mag;
		if (Float.isNaN(nx)||Float.isNaN(ny)||Float.isNaN(nz)){
			/*System.out.println("nx: "+nx);
			System.out.println("ny: "+ny);
			System.out.println("nz: "+nz);
			System.out.println("vx: "+vx);
			System.out.println("vy: "+vy);
			System.out.println("vz: "+vz);
			System.out.println("ux: "+ux);
			System.out.println("uy: "+uy);
			System.out.println("uz: "+uz);*/
		}
		sd=dot(nx,ny,nz, nx,ny,nz);
		
		
		d0 = -dot(nx, ny, nz, x1, y1, z1);
		m = new PMatrix3D();


		float px = 0;
		float py = 0;
		float pz = 1;
		// if ()
		float angle = (float) Math.acos(dot(nx, ny, nz, px, py, pz));
		float uVx = ny * pz - nz * py;
		float uVy = nz * px - nx * pz;
		float uVz = nx * py - ny * px;

		// if same angle, u=0;
		// if (u.mag()<0.00001)System.out.println("small");


		float absAngle = Math.abs(angle);
		if (absAngle > 0.001f && absAngle < Math.PI-0.001f) {
			float magU = getLength(uVx, uVy, uVz);
			uVx /= magU;
			uVy /= magU;
			uVz /= magU;
			float c = (float) Math.cos(angle);
			float s = (float) Math.sin(angle);
			float t = 1f - c;
			m.m00 =(float)( c + uVx * uVx * t);
			m.m01 =(float)( uVx * uVy * t - uVz * s);
			m.m02 = (float)(uVx * uVz * t + uVy * s);
			m.m10 = (float)(uVy * uVx * t + uVz * s);
			m.m11 = (float)(c + uVy * uVy * t);
			m.m12 = (float)(uVy * uVz * t - uVx * s);
			m.m20 = (float)(uVz * uVx * t - uVy * s);
			m.m21 = (float)(uVz * uVy * t + uVx * s);
			m.m22 = (float)(c + uVz * uVz * t);
		} else {
			float dZ=Math.abs(z1-z2)+Math.abs(z3-z2)+Math.abs(z3-z1);
			
			if (dZ>0.0001){
				/*System.out.println("not in XY Plane: "+dZ);
				System.out.println("nx: "+nx);
				System.out.println("ny: "+ny);
				System.out.println("nz: "+nz);*/
			}
		}
		
		float[] f1 = new float[] {(float) x1, (float) y1,(float)  z1 };
		float[] f2 = new float[] {(float)  x2, (float) y2,(float)  z2 };
		float[] f3 = new float[] { (float) x3, (float) y3,(float)  z3 };

		float[] f1T = m.mult(f1, null);
		float[] f2T = m.mult(f2, null);
		float[] f3T = m.mult(f3, null);

		l1 = new MSegment(f1T[0], f1T[1], f1T[2], f2T[0], f2T[1], f2T[2]);
		l2 = new MSegment(f2T[0], f2T[1], f2T[2], f3T[0], f3T[1], f3T[2]);
		l3 = new MSegment(f3T[0], f3T[1], f3T[2], f1T[0], f1T[1], f1T[2]);
	}




	public float getDistanceExtrude(float x, float y, float z) {
		float minD = Float.MAX_VALUE;
		float[] pt = new float[] {(float) x,(float) y, (float)z };
		float[] trans = new float[3];

		trans = m.mult(pt, null);
		float cX = trans[0];
		float cY = trans[1];
		float cZ = trans[2];

		int r1 = Line2D.relativeCCW(l1.x1, l1.y1, l1.x2, l1.y2, cX, cY);
		int r2 = Line2D.relativeCCW(l2.x1, l2.y1, l2.x2, l2.y2, cX, cY);
		int r3 = Line2D.relativeCCW(l3.x1, l3.y1, l3.x2, l3.y2, cX, cY);
		float cDistance;
		if (r1 == r2 && r1 == r3) {
			cDistance = (cZ - l1.z2) * (cZ - l1.z2);
			return (float) Math.sqrt(cDistance);
		}
		return minD;
	}

	
	
	public float getDistanceSqr(float x, float y, float z) {
		float minD = Float.MAX_VALUE;
		float[] pt = new float[] { x, y, z };
		
		// tranlate point
		float[] trans = new float[3];
		trans = m.mult(pt, null);
		float cX = trans[0];
		float cY = trans[1];
		float cZ = trans[2];

		
		// check if inside our outside
		int r1 = Line2D.relativeCCW(l1.x1, l1.y1, l1.x2, l1.y2, cX, cY);
		int r2 = Line2D.relativeCCW(l2.x1, l2.y1, l2.x2, l2.y2, cX, cY);
		int r3 = Line2D.relativeCCW(l3.x1, l3.y1, l3.x2, l3.y2, cX, cY);
		float cDistance;
		if (r1 == r2 && r1 == r3) {
			cDistance = (cZ - l1.z2) * (cZ - l1.z2);
			if (cDistance < minD) {
				minD = cDistance;
			}
		} 
		
		// find closest distance to line 
		else {
			float d1 = (float) l1.distToSegSqr(cX, cY, cZ);
			float d2 = (float) l2.distToSegSqr(cX, cY, cZ);
			float d3 = (float) l3.distToSegSqr(cX, cY, cZ);
			if (d1 < minD) {
				minD = d1;
			}
			if (d2 < minD) {
				minD = d2;
			}
			if (d3 < minD) {
				minD = d3;
			}
		}

		return (float) minD;
	}

	
	public float getDistance(float x, float y, float z) {
		// TODO Auto-generated method stub
		return (float) Math.sqrt(getDistanceSqr(x, y, z));
	}

	
	// not sure if this is right
	public float getDistancePlane(float x, float y, float z) {
		return d0 + dot(nx, ny, nz, x, y, z);

	}
	
	//http://geomalgorithms.com/a04-_planes.html
	public float getDistanceToPlane(float x,float y,float z){
		float    sb, sn;
	    sn = -dot( nx,ny,nz, x-x1,y-y1,y-z1);
	    sb = sn / sd;
	    float rX=sb*nx;
	    float rY=sb*ny;
	    float rZ=sb*nz;
	    return getLength(rX,rY,rZ);
	}

	
	

	public void calculateBounds(float[] bounds) {
		bounds[0] = x1;
		bounds[1] = y1;
		bounds[2] = z1;
		bounds[3] = x1;
		bounds[4] = y1;
		bounds[5] = z1;

		bounds[0] = Math.min(bounds[0], x2);
		bounds[0] = Math.min(bounds[0], x3);
		bounds[1] = Math.min(bounds[1], y2);
		bounds[1] = Math.min(bounds[1], y3);
		bounds[2] = Math.min(bounds[2], z2);
		bounds[2] = Math.min(bounds[2], z3);

		bounds[3] = Math.max(bounds[3], x2);
		bounds[3] = Math.max(bounds[3], x3);
		bounds[4] = Math.max(bounds[4], y2);
		bounds[4] = Math.max(bounds[4], y3);
		bounds[5] = Math.max(bounds[5], z2);
		bounds[5] = Math.max(bounds[5], z3);
	}
	
	public int triIntersect(float px, float py, float pz, float dx, float dy,
			float dz, float[] intersection) {

		float w0x = px - x1;
		float w0y = py - y1;
		float w0z = pz - z1;

		float a = -dot(nx, ny, nz, w0x, w0y, w0z);
		float b = dot(nx, ny, nz, dx, dy, dz);

		if (Math.abs(b) < SMALL_NUM) { // ray is parallel to triangle plane
			if (a == 0) // ray lies in triangle plane
				return SAME_PLANE;
			else
				return DISJOINT; // ray disjoint from plane
		}

		float r = a / b;
		if (r < 0.0) // ray goes away from triangle
			return DISJOINT; // => no intersect

		intersection[0] = px + r * dx;
		intersection[1] = py + r * dy;
		intersection[2] = pz + r * dz;

		float wx = intersection[0] - x1;
		float wy = intersection[1] - y1;
		float wz = intersection[2] - z1;

		float wu = dot(wx, wy, wz, ux, uy, uz);
		float wv = dot(wx, wy, wz, vx, vy, vz);

		// get and test parametric coords

		float s = (uv * wv - vv * wu) / d;
		if (s < 0.0 || s > 1.0) // I is outside T
			return DISJOINT;
		float t = (uv * wu - uu * wv) / d;
		if (t < 0.0 || (s + t) > 1.0) // I is outside T
			return DISJOINT;
		return INTERSECT; // I is in T
	}

	public int triIntersect(float px, float py, float pz, float dx, float dy,
			float dz, float x1, float y1, float z1, float x2, float y2,
			float z2, float x3, float y3, float z3, float[] intersection) {
		float ux = x2 - x1;
		float uy = y2 - y1;
		float uz = z2 - z1;
		float vx = x3 - x1;
		float vy = y3 - y1;
		float vz = z3 - z1;

		float nx = uy * vz - uz * vy;
		float ny = uz * vx - ux * vz;
		float nz = ux * vy - uy * vx;

		float w0x = px - x1;
		float w0y = py - y1;
		float w0z = pz - z1;

		float a = -dot(nx, ny, nz, w0x, w0y, w0z);
		float b = dot(nx, ny, nz, dx, dy, dz);

		if (Math.abs(b) < SMALL_NUM) { // ray is parallel to triangle plane
			if (a == 0) // ray lies in triangle plane
				return SAME_PLANE;
			else
				return DISJOINT; // ray disjoint from plane
		}

		float r = a / b;
		if (r < 0.0) // ray goes away from triangle
			return DISJOINT; // => no intersect

		intersection[0] = px + r * dx;
		intersection[1] = py + r * dy;
		intersection[2] = pz + r * dz;

		float uu = dot(ux, uy, uz, ux, uy, uz);
		float uv = dot(ux, uy, uz, vx, vy, vz);
		float vv = dot(vx, vy, vz, vx, vy, vz);

		float wx = intersection[0] - x1;
		float wy = intersection[1] - y1;
		float wz = intersection[2] - z1;

		float wu = dot(wx, wy, wz, ux, uy, uz);
		float wv = dot(wx, wy, wz, vx, vy, vz);
		float d = uv * uv - uu * vv;

		// get and test parametric coords

		float s = (uv * wv - vv * wu) / d;
		if (s < 0.0 || s > 1.0) // I is outside T
			return DISJOINT;
		float t = (uv * wu - uu * wv) / d;
		if (t < 0.0 || (s + t) > 1.0) // I is outside T
			return DISJOINT;
		return INTERSECT; // I is in T
	}

	public int triIntersect2(float px, float py, float pz, float dx, float dy,
			float dz, float x1, float y1, float z1, float x2, float y2,
			float z2, float x3, float y3, float z3, float[] intersection) {
		float e1x = x2 - x1;
		float e1y = y2 - y1;
		float e1z = z2 - z1;
		float e2x = x3 - x1;
		float e2y = y3 - y1;
		float e2z = z3 - z1;

		float pVecX = dy * e2z - dz * e2y;
		float pVecY = dz * e2x - dx * e2z;
		float pVecZ = dx * e2y - dy * e2x;

		float det = dot(e1x, e1y, e1z, pVecX, pVecY, pVecZ);
		if (det > -SMALL_NUM && det < SMALL_NUM)
			return DISJOINT;
		float inv_det = 1f / det;

		float tVecX = px - x1;
		float tVecY = py - y1;
		float tVecZ = pz - z1;

		float u = dot(tVecX, tVecY, tVecZ, pVecX, pVecY, pVecZ) * inv_det;

		if (u < 0 || u > 1)
			return DISJOINT;

		float qVecX = tVecY * e1z - tVecZ * e1y;
		float qVecY = tVecZ * e1x - tVecX * e1z;
		float qVecZ = tVecX * e1y - tVecY * e1x;

		float v = dot(dx, dy, dz, qVecX, qVecY, qVecZ) * inv_det;
		if (v < 0.0 || u + v > 1.0)
			return DISJOINT;
		float t = dot(e2x, e2y, e2z, qVecX, qVecY, qVecZ) * inv_det;
		intersection[0] = px + t * dx;
		intersection[1] = py + t * dy;
		intersection[2] = pz + t * dz;
		return INTERSECT;
	}

	public float ptSegDistSq(float[] seg, float x, float y, float z) {
		float vx = x2 - x1;
		float vy = y2 - y1;
		float vz = z2 - z1;
		float c2 = dot(vx, vy, vz, vx, vy, vz);
		float dx = x - x1;
		float dy = y - y1;
		float dz = z - z1;
		float c1 = dot(dx, dy, dz, vx, vy, vz);
		if (c1 <= 0)
			return distSqr(x, y, z, x1, y1, z1);
		if (c2 <= c1)
			return distSqr(x, y, z, x2, y2, z2);
		float b = c1 / c2;
		float px = x1 + b * vx;
		float py = y1 + b * vy;
		float pz = z1 + b * vz;
		return distSqr(x, y, z, px, py, pz);
	}

	public float distSqr(float x1, float y1, float z1, float x2, float y2,
			float z2) {
		float dX = x2 - x1;
		float dY = y2 - y1;
		float dZ = z2 - z1;
		return dX * dX + dY * dY + dZ * dZ;
	}

}
